'
' Central Pattern Engine
'
' This is a central pattern generator with
' a state engine, an internal clock, and calibration settings.
' It can switch between wave forms for servos seemlessly, speed up
' slow down, and go backward.


' Parameters of the gait mode
' Automatic heart rate, a rate is tied to one of the throttle settings

' One pulse channel only
' Two throttles: x throt, y thort
' Pin 1-16
' The pulse channel can be linked to either throttle, its period goes from max to - max (inverse wave)
' Each pin is linked to pulse channel with a range (which could be 0)
' Each pin can have its min and max point increased or decreased by a throttle

' So to turn you would have the pulse channel linked to the y throttle
' all horizontal and vertical servos would have their motion linked to the pulse channel
' The horizontal servos would have their range increased or decreased by the x throttle

' To stand and lean, all servos would have zero influence from the pulse channel
' their range would be 1, but their min and max would be set by the x or y throttle.  

OBJ
  ROSC : "ROSC"  ' Robot OS Constants
  SERVO : "Servo32v7"
  I2C : "Basic_i2c_Driver"

  ' for debugging
#ifdef _CPE_debug_
  TV      : "TV_Text"
#endif


CON
  GUN_PIN = 18
  POSE_OFF = 0
  POSE_ON = 1



VAR
  byte buffer[ROSC#PAGE_SIZE]

  long servos
  long x_throt
  long y_throt
  long direction
  long delay  ' for speed
  long cp_deg, stride_count ' where I am at
  long left_stroke, right_stroke, left_lift, right_lift

  byte joint[ROSC#MAX_SERVOS]    ' what kind of joint is it: left, right, hip, knee, shoulder
  byte servo_pin[ROSC#MAX_SERVOS]
' word low_range[ROSC#MAX_SERVOS]
' word high_range[ROSC#MAX_SERVOS]
  word func_range[ROSC#MAX_SERVOS]  ' combine range 12 bits, with function in top 4 bits
  word base[ROSC#MAX_SERVOS]

  word phase_channel[ROSC#MAX_SERVOS]  ' how plus or minus am I        12 bits phase, 4 bits channel
  byte counter_channel[ROSC#MAX_SERVOS]


  byte servo_count
  long stack[2048]

  byte cog

  long servo_pose[ROSC#MAX_SERVOS] ' set the pose to this amount
  byte pose_status[ROSC#MAX_SERVOS]

   
PUB Init | loop

#ifdef _CPE_debug_
  TV.start(24)    
                          

  TV.out($00) ' Clear screen
  
  TV.str(string("CPE "))
  TV.out(13)
  TV.out(32)
  TV.out(13)

#endif
  waitcnt(cnt+CLKFREQ)

  LoadServoSettings                    

  SetCycleStride(0, 0)
  
{
     repeat loop from 0 to ROSC#MAX_SERVOS-1
       TV.dec(servo_pin[loop])
       TV.out(32)
       TV.dec(base[loop])
       TV.out(32)
 }

  ' initialize servo position and function




  
  repeat loop from 0 to servo_count-1
    Servo.set(servo_pin[loop], 1500)
    servo_pose[loop]:=1500
    pose_status:=POSE_OFF

  Servo.start


PUB StartGo 
  cog:=cognew(go, @stack)
  return cog


PUB Stop
  cogstop(cog)



PUB PoseOn | loop ' later pass values
  repeat loop from 0 to ROSC#servo_count-1
    pose_status:=POSE_ON

PUB PoseOff
  repeat loop from 0 to ROSC#servo_count-1
    pose_status:=POSE_OFF




PUB DebugVal(n)
#ifdef _CPE_debug_
  TV.str(string("Debug Val "))
  TV.dec(n)
  TV.out(13)
#endif

PUB SetDirection(din)
  direction:=din

PUB SetSpeed(din)
  delay:=din

PUB SetServoCount(cin)
  servo_count:=cin  


PUB SetPose(pose_ptr)

  
PUB Go | ldeg, rdeg
  {
    Convert x, y into s direction and speed.  
  }

  {
    I need a counter channel that goes from 1-8 incrementing every 180 degrees.  
    A counter mask (byte) which checks if it is active
  }
  ldeg:=0
  rdeg:=0
#ifdef _CPE_debug_
  TV.str(string("GO Starts / DEBUG Defined "))
  TV.out(13)


  TV.str(string("left degree "))
            
  TV.dec(ldeg)
  TV.str(string(" right "))
  TV.dec(rdeg)
  TV.out(13)
      
  TV.str(string("Delay "))
  TV.dec(delay)
  TV.out(13)

  waitcnt(cnt+CLKFREQ*3)
#endif
  repeat
  


#ifdef _CPE_debug_


#endif
      if ((direction & ROSC#LEFT_FWD)== 0)
        ldeg:=cp_deg
#ifdef _CPE_debug_
        TV.str(string("LEFT FORWARD "))
        TV.out(13)
#endif

      if ((direction & ROSC#RIGHT_FWD)== 0)
        rdeg:=cp_deg 
#ifdef _CPE_debug_
        TV.str(string("RIGHT FORWARD "))
        TV.out(13)
#endif

      if ((direction & ROSC#LEFT_BWD)==0)
        ldeg:=359-cp_deg
#ifdef _CPE_debug_
        TV.str(string("LEFT BACKWARD "))
        TV.out(13)
#endif

      if ((direction & ROSC#RIGHT_BWD)==0 )
        rdeg:=359-cp_deg 
#ifdef _CPE_debug_
        TV.str(string("RIGHT BACKWARD "))
        TV.out(13)
#endif
      {
        check a bit mask against a bit Shifted right stride count # of times
        if it is true, set the servos (otherwise frozen)
      }

      SetServos(ldeg, rdeg, stride_count)


      if (delay>0)  ' otherwise I want to freeze
        cp_deg:=cp_deg+5
        if (cp_deg==180)
          stride_count:=stride_count+1
        if cp_deg>359
          stride_count:=stride_count+1
          cp_deg:=0
        if (stride_count>3)
          stride_count:=0  
        waitcnt(cnt+delay)  ' some number for speed

PUB StopAim
  SERVO.Set(GUN_PIN, 1000)
  

PUB DoAim(v, h)  | loop' total hack to do aiming
  SERVO.Set(GUN_PIN, 1900)
  repeat loop from 0 to ROSC#MAX_SERVOS
     SERVO.Set(loop, 1500)  

      if (joint[loop] & ROSC#FRONT)
         if (joint[loop] & ROSC#VERT)
           if (joint[loop] & ROSC#RIGHT)
              ' set it up or down                  
           else
              ' set it up or down
      else
        if (joint[loop] & ROSC#HORIZ)
           if (joint[loop]  & ROSC#RIGHT)
           else 
   

PUB SetCycleStride(cin, stin)
  cp_deg:=cin
  stride_count:=stin


PUB GetServoTarget(loop, ldeg, rdeg)  | target, phase, range, deg_filt
    range:=func_range[loop] & (!ROSC#ALL_FUNC) ' (high_range[loop]-low_range[loop])/2 ' should do a shift right
    phase:=phase_channel[loop] & (!ROSC#GAIT_PHASE_ALL) ' get the gait masks out
   
    CASE (func_range[loop] & ROSC#ALL_FUNC)           ' the function
      ROSC#COS_FUNC : 
        if (joint[loop] & ROSC#RIGHT)
          target:= base[loop]+cos(rdeg + phase, range)
        else
          target:= base[loop]+cos(ldeg + phase, range)
        
     ROSC#SIN_FUNC :
        if (joint[loop] & ROSC#RIGHT)
          if  ((direction & ROSC#ANY_RIGHT_DIR) OR (joint[loop] & ROSC#VERT))
            target:= base[loop]+sin(rdeg + phase, range)
          else
            target:= base[loop]  
        else
          if ((direction & ROSC#ANY_LEFT_DIR) OR (joint[loop] & ROSC#VERT))
            target:= base[loop]+sin(ldeg + phase, range)
          else
            target:= base[loop]  

          
     ROSC#STATIC_MID_FUNC:
        target:=base[loop] ' MID
     ROSC#STATIC_HIGH_FUNC:
        if (joint[loop] & ROSC#RIGHT)     
          deg_filt:=rdeg
        else
          deg_filt:=ldeg
          
        deg_filt:=deg_filt+phase
        deg_filt:=deg_filt//360
        
            
        deg_filt:=deg_filt*10-1800
        ' clamp it
        if (deg_filt<90)
          deg_filt:= 90
        if (deg_filt>270)
          deg_filt:=270  
        
        target:=base[loop]+sin(deg_filt, range)
        
       {   
       if (joint[loop] &ROSC#RIGHT)
           if (sin(rdeg, range)>0)
             target:=target+range
         if (joint[loop]& ROSC#RIGHT)
           if (sin(ldeg, range)>0)
              target:=target+range                        
       }
  {        
    ' pitch
    if (joint[loop]== (RIGHT | VERT))
        target:=target +tilt '+ r_tilt
        
    if (joint[loop]== (LEFT | VERT))
        ' -50 is the even out factor
       target:=target-SERVO_ALIGNMENT_ADJUSTMENT - tilt '+ l_tilt
    }


      {target:= base[loop]+sin(rdeg + phase_channel[loop], range) }


#ifdef _CPE_debug_
    TV.dec(phase_channel[loop])
    TV.dec(target)
    TV.out(32)
#endif
  return target

PUB SetServos(ldeg, rdeg, stride) | loop,  target ' Set Servos to the appropriate postion for a raidan
  ' so far I have only done horizontal vertical for sine

  repeat loop from 0 to servo_count-1
   'loop:=5 ' Test servo

   ' test to see if this is the right stride to move

    case(stride)
        0: if (phase_channel[loop] & ROSC#GAIT_PHASE1 == 0)
              Next
        1: if (phase_channel[loop] & ROSC#GAIT_PHASE2 == 0)
              Next
        2: if (phase_channel[loop] & ROSC#GAIT_PHASE3 == 0)
              Next
        3: if (phase_channel[loop] & ROSC#GAIT_PHASE4 == 0)
              Next








   

    target:=GetServoTarget(loop, ldeg, rdeg)


    ' here put in the set pose/ unset pose part

    if (counter_channel[loop]<>99)                   ' counter motion
      target:=target+ (1500- GetServoTarget(counter_channel[loop], ldeg, rdeg ))    
   

    if (pose_status[loop]==POSE_ON)
      target:=servo_pose[loop]

    SERVO.Set(servo_pin[loop], target)

  'TV.dec(stride)    



PUB sin(degree, range) : s | c,z,angle
  angle := (degree*91)~>2  ' *22.75
  c := angle & $800
  z := angle & $1000
  if c
    angle := -angle
  angle |= $E000>>1
  angle <<= 1
  s := word[angle]
  if z
    s := -s
  return (s*range)~>16     ' return sin = -range..+range

PUB cos(degree,range)
  return sin(degree+90,range)



PUB Calibrate
   ' unlike Go, you set the servos

PUB SaveServoSettings      | loop


  
PUB LoadServoSettings   | loop
  ' Load the servo pin and joint
  ' Load the servo pin and joint
  readPage($8000)      

  repeat loop from 0 to 11
    joint[loop]:=BYTE[@buffer+1][loop]
    servo_pin[loop]:=BYTE[@buffer+1][loop + 12]
#ifdef _CPE_debug_
    TV.dec(servo_pin[loop])
    TV.out(32)
#endif

  readPage($8000+32)
  repeat  loop from 0 to 11
    func_range[loop]:=WORD[@buffer+2][loop]

#ifdef _CPE_debug_
    TV.dec((func_range[loop] & (!ROSC#ALL_FUNC)) )
    TV.out(32)
#endif

  readPage($8000+64)

#ifdef _CPE_debug_
  repeat loop from 0 to 31
    TV.dec(buffer[loop])
    TV.out(32)
#endif

  repeat loop from 0 to 11
    base[loop]:=WORD[@buffer+2][loop]
#ifdef _CPE_debug_
    TV.dec(base[loop])
    TV.out(32)
#endif


      
  readPage($8000+96)
  repeat loop from 0 to 11
    phase_channel[loop]:=WORD[@buffer+2][loop]
                          
  readPage($8000+128)
  repeat loop from 0 to ROSC#MAX_SERVOS-1
    counter_channel[loop]:=BYTE[@buffer+1][loop]
  
  ' Next page
  'load the func range
  ' next pin
  ' load the base



  ' all I really need now are func_range and phase * 12 = 24 longs



PRI readPage(eepromAddress)
  if i2c.ReadPage(i2c#BootPin, i2c#EEPROM, eepromAddress, @buffer, 32)
     abort ' an error occurred during the read

PRI writePage(eepromAddress) | startTime
  if i2c.WritePage(i2c#BootPin, i2c#EEPROM, eepromAddress, @buffer, 32)
    abort ROSC#BAD_WRITE ' an error occured during the write
  startTime := cnt ' prepare to check for a timeout
  repeat while i2c.WriteWait(i2c#BootPin, i2c#EEPROM, eepromAddress)
  if cnt - startTime > clkfreq / 10
    abort ROSC#BAD_WRITE' waited more than a 1/10 second for the write to finish
